#ifndef RSIM_EOMDATA_H
#define RSIM_EOMDATA_H

/* -------------------------------------------------------------------------- *
 * File: EOMData.h							      *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                      					      *
 * Email: 								      *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *									      *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)			      *
 * -------------------------------------------------------------------------- */

#include "LinAlgDecl.h"

namespace RSIM{

class Transform;

class EOMData{
	public:
		EOMData();
		
		~EOMData();
		
		/** clears memory and sets all pointer values to NULL */
		void clear();
		
		void initEOM(const double& Mass,const Mat3x3& Inertia, const Transform& T_BF);
		
		Mat6x6* wgetdMk1();
		Mat3x3* wgetJk1();
		Vect6* wgetAk1();
		Vect6* wgetFkc1();
		Vect6* wgetdFk1();
		
		const Mat6x6* getdMk1()const;
		const Mat3x3* getJk1()const;
		const Vect6* getAk1()const;
		const Vect6* getFkc1()const;
		const Vect6* getdFk1()const;

	private:
		/** Spatial mass matrix shifted to frame F */
		Mat6x6 *dMk1_;
		
		/** Jk1 = Inertia - mass * [rx][rx]
		Inertia and rx are both written in frame F */
		Mat3x3 *Jk1_;
		
		/** Spatial acceleration of frame F expressed in F*/
		Vect6 *Ak1_;
		
		/** Spatial constraint force acting on frame F of this body exprssed in F. 
		These values normally lie in the non-working constraint force subspace of F if there
		is no prescribed motion acting on the joint connecting this body to parent. If 
		we have a prescribed motion, then potentially, all 6 values of this vector could be
		nonzero. */
		Vect6 *Fkc1_;
		
		/** Equation of motion of this body expressed in F is dMk1_ * Ak1_ -dFk1_ = Constraint forces shifted to F.
		All quantities are expressed in frame F*/
		Vect6 *dFk1_;
		
		
		
}; // class EOMData

inline Mat6x6* EOMData::wgetdMk1(){return dMk1_;}
inline const Mat6x6* EOMData::getdMk1()const{return dMk1_;}

inline Mat3x3* EOMData::wgetJk1(){return Jk1_;}
inline const Mat3x3* EOMData::getJk1()const{return Jk1_;}

inline Vect6* EOMData::wgetAk1(){return Ak1_;}
inline const Vect6* EOMData::getAk1()const{return Ak1_;}

inline Vect6* EOMData::wgetFkc1(){return Fkc1_;}
inline const Vect6* EOMData::getFkc1()const{return Fkc1_;}

inline Vect6* EOMData::wgetdFk1(){return dFk1_;}
inline const Vect6* EOMData::getdFk1()const{return dFk1_;}

} // namespace RSIM

#endif